#include "Code0_MAIN.h"

Bool CallInitialize = FALSE;
extern Bool Zigbee_Disable;
											
/*******************************************************************************
* Function Name  : Initialize
* Description    : Initialize main variable and enable Zigbee
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/ 
void Initialize(void)
{										 	
	MotorSchedule_Init();	
	HeadSchedule_Init();
  Motion_Init();
	Head_Init();	 
	MotorRX_Init();
	ZigbeeRX_Init();		
	MotorBeWrited = FALSE;
	HeadBeWrited = FALSE;
	Zigbee_Disabled = FALSE; 
//	Zigbee_SendMotorData = FALSE;
					
	#ifdef DEBUG_PORT
		USART_ITConfig(DEBUG_PORT, USART_IT_RXNE, ENABLE);	 
	#endif
	CallInitialize = FALSE;
}
			
/*******************************************************************************
* Function Name  : RenewReadID
* Description    : 1. Renew the ID of the motor which will be read.	
									 		High priority: 0 ~ 2
									 		Low priority: the others
									 2. Update the battery power of the motor. 
* Input          : None
* Output         : None
* Return         : None
* Attention		 : Reading packet must not interrupt the writing packet. 
*******************************************************************************/ 
void RenewReadID(void)
{	
	int temp_part, temp_no;																			
	static Bool read_main = TRUE;		
	static unsigned char id_index = 0;			 	
	BodyPartStatus bodypart = UPPER_BODY;
	static unsigned char body_id_index_main = 0;
	static unsigned char body_id_index_other = 5;
	static unsigned char send_master_count = 0;

	if((MotorWritedFinish[UPPER_BODY] == TRUE) && 
		 (MotorWritedFinish[RIGHT_LEG] == TRUE) &&
		 (MotorWritedFinish[LEFT_LEG] == TRUE))
	{
		if(read_main)
		{							
			id_index = body_id_index_main;
			ReadMotor(id_index);
			body_id_index_main++;

			if(body_id_index_main > 4)
			{
				body_id_index_main = 0;
			}		 
			CompSend();
		}
		else
		{							
			id_index = body_id_index_other;											
			ReadMotor(id_index);
			body_id_index_other++;

			if(body_id_index_other > MOTOR_NUM)
			{
				body_id_index_other = 5;
			}	 
		}
		
		IDtoPartNo(MotorData.Read[id_index][READ_ID], &temp_part, &temp_no);
		bodypart = temp_part;					

		if(MotorData.Read[id_index][READ_VOLTAGE] > 0)	
		{
			MotorData.Battery[bodypart] = MotorData.Read[id_index][READ_VOLTAGE];	 
			MotorData.BatteryCount[bodypart] = 10;	
		}	
		else if(MotorData.BatteryCount[bodypart] > 0)
		{																								
			MotorData.BatteryCount[bodypart]--;
		}	
		if(MotorData.BatteryCount[bodypart] == 0)
		{											 		
			MotorData.Battery[bodypart] = 0;	
		}

		read_main = !read_main;
	}


	BtnTriggeredEvent();
	if(Read_IMU)
	{
		EXTI_GenerateSWInterrupt(EXTI_Line5);
		//IMU_Run();
		//RM146();
	}
//	IMU_Run();
//	RM146();

	//printf("%d \r\n", (int)azimuth[1]);
//	
//	USART_SendData(DEBUG_PORT,( (unsigned char)azimuth[1]/100));    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}															
//	USART_SendData(DEBUG_PORT, ((unsigned char)azimuth[1]%100));    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
}
/*******************************************************************************
* Function Name  : RenewReadIMU
* Description    : Renew the IMU measurement data which will be read.
                   and reserve data in Motor.Read array.	
* Input          : None
* Output         : None
* Return         : None
* Attention		 : Reading packet must not interrupt the writing packet. 
*******************************************************************************/ 
void RenewReadIMU(void)
{	


static unsigned char Acc_index = 27;
static unsigned char Mag_index = 28;
static unsigned char Gyro_index = 29;

IMU_RawData();

MotorData.Read[Acc_index][READ_ID] =            51;
MotorData.Read[Acc_index][READ_LOW_POSITION] =  Acc.Buff[0];    // X axis  low_byte data
MotorData.Read[Acc_index][READ_HIGH_POSITION] = Acc.Buff[1];		// X axis hign_byte data
MotorData.Read[Acc_index][READ_LOW_LOADING] =   Acc.Buff[2];		// Y axis  low_byte data
MotorData.Read[Acc_index][READ_HIGH_LOADING] =  Acc.Buff[3];		// Y axis hign_byte data
MotorData.Read[Acc_index][READ_VOLTAGE] =       Acc.Buff[4];		// Z axis  low_byte data
MotorData.Read[Acc_index][READ_RESERVE] =       Acc.Buff[5];		// Z axis hign_byte data

MotorData.Read[Mag_index][READ_ID] =            52;
MotorData.Read[Mag_index][READ_LOW_POSITION] =  Mag.Buff[0];    // X axis  low_byte data
MotorData.Read[Mag_index][READ_HIGH_POSITION] = Mag.Buff[1];		// X axis hign_byte data
MotorData.Read[Mag_index][READ_LOW_LOADING] =   Mag.Buff[2];		// Y axis  low_byte data
MotorData.Read[Mag_index][READ_HIGH_LOADING] =  Mag.Buff[3];		// Y axis hign_byte data
MotorData.Read[Mag_index][READ_VOLTAGE] =       Mag.Buff[4];		// Z axis  low_byte data
MotorData.Read[Mag_index][READ_RESERVE] =       Mag.Buff[5];		// Z axis hign_byte data

MotorData.Read[Gyro_index][READ_ID] =            53;
MotorData.Read[Gyro_index][READ_LOW_POSITION] =  Gyro.Buff[0];    // X axis  low_byte data
MotorData.Read[Gyro_index][READ_HIGH_POSITION] = Gyro.Buff[1];		// X axis hign_byte data
MotorData.Read[Gyro_index][READ_LOW_LOADING] =   Gyro.Buff[2];		// Y axis  low_byte data
MotorData.Read[Gyro_index][READ_HIGH_LOADING] =  Gyro.Buff[3];		// Y axis hign_byte data
MotorData.Read[Gyro_index][READ_VOLTAGE] =       Gyro.Buff[4];		// Z axis  low_byte data
MotorData.Read[Gyro_index][READ_RESERVE] =       Gyro.Buff[5];		// Z axis hign_byte data



 /* if(ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC)==SET)
	
        AD_value=ADC_GetConversionValue(ADC1);   */

}
/*******************************************************************************
* Function Name  : RenewMotion
* Description    : 1. Update HeadSchedule and MotorSchedule to the MotorData	
									 2. Check the packet received from Master 
									 3. Renew Motion
									 4. Send the information of motor to Master
* Input          : None
* Output         : None
* Return         : None
* Attention		 : Reading packet must not interrupt the writing packet. 
*******************************************************************************/ 
void RenewMotion(void)
{					
//	unsigned int i = 0;		
//	
//	i = TIM_GetCounter(MOTOR_WRITE_TIMER);			
//	USART_SendData(DEBUG_PORT, (i / 100));
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}																							
//	USART_SendData(DEBUG_PORT, (i % 100));
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}														
//					 	
//	TIM_SetCounter(MOTOR_WRITE_TIMER, 0x0000); 
	
	// Do motion & head motion
	if(Head.Mode_RX > 0)
	{
		Head.Mode = Head.Mode_RX;	
		Head.Mode_RX = 0;			
	}	
	
	HeadMotion();	
	if(HeadSchedule.NowIndex != HeadSchedule.EndIndex)
	{							
		Update_HeadSchedule_to_MotorData();
	}
						
  Update_MotorSchedule_to_MotorData();	
	if((HeadBeWrited == TRUE) && (MotorBeWrited == FALSE))
	{																			
		WriteMotor(UPPER_BODY);
		HeadBeWrited = FALSE;
	}						


	if(((CompRX.NowIndex + COMP_R_LENGTH) % COMP_RX_BUFF_SIZE) <= CompRX.EndIndex)
	{
		if(CompRX2Receive() == TRUE)
		{ 												 
			LedSwitch(!GetLedStatus(4), 4);
		}
	}
	if(CallInitialize == TRUE)
	{
		Initialize();
		Mtn_POSE_Standing(128, 0, 17);	
	}									

//	// Do motion & head motion
//	if(Head.Mode_RX > 0)
//	{
//		Head.Mode = Head.Mode_RX;	
//		Head.Mode_RX = 0;			
//	}	
//	
//	HeadMotion();	
//	if(HeadSchedule.NowIndex != HeadSchedule.EndIndex)
//	{							
//		Update_HeadSchedule_to_MotorData();
//	}

	if((MotorSchedule.NowIndex == MotorSchedule.EndIndex) && (MotorSchedule.Delay_ExecutedCount == 0))
	{
		if(Motion.Mode_RX >= 100)
		{
			Motion.Mode = Motion.Mode_RX;
			Motion.ContinueAngle = Motion.ContinueAngle_RX;															 
			Motion.ContinueDistance = Motion.ContinueDistance_RX;
			Motion.Continue_Eanble = TRUE;					 
			Motion.Continue_Set = Motion.Mode;
			Motion.Mode_RX = 0;
		}
		else if((Motion.Mode_RX > 0) && (Motion.Mode_RX < 100))
		{																								 
			Motion.Mode = Motion.Mode_RX;
			Motion.ContinueAngle = Motion.ContinueAngle_RX;															 
			Motion.ContinueDistance = Motion.ContinueDistance_RX;
			Motion.Continue_Eanble = FALSE;					 
			Motion.Continue_Set = OFF;
			Motion.Mode_RX = 0;
		}

		if((Motion.Continue_Eanble == TRUE) || (Motion.Mode >= 100))
		{							
			ContinuousMotion();				
		}
		else
		{
			SingleMotion();		
		}	

//		Update_MotorSchedule_to_MotorData();
//		#ifdef SCHEDULE_UPDATE_TIMER
//			TIM_SetCounter(SCHEDULE_UPDATE_TIMER, 0x0000);
//		  TIM_Cmd(SCHEDULE_UPDATE_TIMER, ENABLE);
//		#endif
	}
	else
	{				 	
		CompRX.NowIndex = 0;
		CompRX.EndIndex = 0;
	}
	
    CompSend();
							 													
//  Update_MotorSchedule_to_MotorData();
//	if((HeadBeWrited == TRUE) && (MotorBeWrited == FALSE))
//	{																			
//		WriteMotor(UPPER_BODY);
//		HeadBeWrited = FALSE;
//	}					
							
}

void Btn1_Triggered(void)
{			
	if(!GPIO_ReadInputDataBit(GPIOF, BTN6) && (!CallInitialize))  
	{
		//LedSwitch(!GetLedStatus(1), 1);			
		LedSwitch(!GetLedStatus(6), 6);
		DipMode = ReadDip() + 100;			
		ReceiceFromComp = FALSE;	
		Comp_Receive_Init();	
		CallInitialize = TRUE;
								 
		#ifdef COMP
			USART_ITConfig(COMP_PORT, USART_IT_RXNE, DISABLE);	
		#endif
	}									

	printf("BTN 1 Triggered\r\n");	
}				

void Btn2_Triggered(void)
{		
	// BTN2 -> Strategy
	DipMode = ReadDip();				 
	ReceiceFromComp = TRUE;	
				
	#ifdef COMP
		USART_ITConfig(COMP_PORT, USART_IT_RXNE, ENABLE);	
	#endif
 
	Read_IMU = TRUE;
	LedSwitch(Read_IMU, 7);
	
	printf("BTN 2 Triggered\r\n");
}

void Btn3_Triggered(void)
{		
	if(ReadDip() == 63)
	{
		Meg_Init();
		Read_IMU = TRUE;		
		LedSwitch(Read_IMU, 7);
		// Nod
		Add_motor_job(  0, 100,   2, 100,   0 );	  
 			Add_motor_job(255,  10,   0,   0,   0 );	 
		Add_motor_job(  0,   0,   2, 100,   0 );	  
	}
	else if(ReadDip() == 62)
	{							 
		Meg.GetMaxMin = FALSE;	   
		Read_IMU = FALSE;		
		LedSwitch(Read_IMU, 7);	
		// Shake head	  
		Add_motor_job(  1, 100,   2, 100,   0 );	  
 			Add_motor_job(255,  10,   0,   0,   0 );	 
		Add_motor_job(  1, 255,   1, 100,   0 );	  
	}
	else if(ReadDip() == 49)
	{
		Read_IMU = !Read_IMU;		
		LedSwitch(Read_IMU, 7);	
	}		
	else if(ReadDip() == 48)
	{
		static int arm = 0;
		//LedSwitch(!GetLedStatus(4), 4);
		if(arm == 0)
		{
			Mtn_Raise_Arm();
			arm = 1;
		}
		else if(arm == 1)
		{
			Mtn_Getdown_Arm();
			arm = 0;
		}								
	}
	
	printf("BTN 3 Triggered\r\n");
}				

void Btn4_Triggered(void)
{
//	ReceiceFromComp = FALSE;	
//	Comp_Receive_Init();
//	Motion.Mode_RX = STOP;				
//	Motion.Mode = STOP;	
//							 
//	#ifdef COMP
//		USART_ITConfig(COMP_PORT, USART_IT_RXNE, DISABLE);	
//	#endif				 

	printf("BTN 4 Triggered\r\n");
}				

void Btn5_Triggered(void)
{
	printf("BTN 5 Triggered\r\n");
}				

void Btn6_Triggered(void)
{
	printf("BTN 6 Triggered\r\n");
}

void BtnTriggeredEvent(void)
{
	ReadBtn();
	
	if(Btn_Triggered[0] == TRUE)
	{
		Btn1_Triggered();
	}

	if(Btn_Triggered[1] == TRUE)
	{
		Btn2_Triggered();
	}							
					 
	if(Btn_Triggered[2] == TRUE)
	{
		Btn3_Triggered();
	}

	if(Btn_Triggered[3] == TRUE)
	{
		Btn4_Triggered();
	}

	if(Btn_Triggered[4] == TRUE)
	{
		Btn5_Triggered();
	}

	if(Btn_Triggered[5] == TRUE)
	{
		Btn6_Triggered();
	}
}